﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.Data;
using System.IO;
using System.Text;
using static MAVLink;

namespace DroneLib.Vehicle.Models
{
    public static class ParamPacks
    {
        static readonly int _Magic = 0x671b;
        static readonly int _MmagicWithDefaults = 0x671c;
        /*
          packed format:
            file header:
              uint16_t magic = 0x671b
              uint16_t num_params
              uint16_t total_params
            per-parameter:
            uint8_t type:4;         // AP_Param type NONE=0, INT8=1, INT16=2, INT32=3, FLOAT=4
            uint8_t flags:4;        // bit 0: includes default value for this param
            uint8_t common_len:4;   // number of name bytes in common with previous entry, 0..15
            uint8_t name_len:4;     // non-common length of param name -1 (0..15)
            uint8_t name[name_len]; // name
            uint8_t data[];         // value, length given by variable type, data length doubled if default is included
            Any leading zero bytes after the header should be discarded as pad
            bytes. Pad bytes are used to ensure that a parameter data[] field
            does not cross a read packet boundary
        */

        static Dictionary<int, (int type_len, char type_format)> map = new Dictionary<int, (int type_len, char type_format)>()
        {
            { 1, (1, 'b') },
            { 2, (2, 'h') },
            { 3, (4, 'i') },
            { 4, (4, 'f') },
        };

        private static List<ParamInfo> Extract0(MemoryStream paramFileStream)
        {
            List<ParamInfo> result = new List<ParamInfo>();

            BinaryReader br = new BinaryReader(paramFileStream);
            var magic = br.ReadUInt16();
            var num_params = br.ReadUInt16();
            var total_params = br.ReadUInt16();

            if (magic != _Magic && magic != _MmagicWithDefaults)
                return null;

            bool with_defaults = magic == _MmagicWithDefaults;


            byte pad_byte = 0;
            int count = 0;
            var last_name = "";
            byte temp;
            while (paramFileStream.Position < paramFileStream.Length)
            {
                temp= br.ReadByte();
                if (temp == 0)
                    continue;

                var ptype = br.ReadByte();
                var plen = br.ReadByte();
                var flags = (ptype >> 4) & 0x0F;
                ptype &= 0x0F;

                if (!map.TryGetValue(ptype, out (int type_len, char type_format) mapped))
                    return null;

                var name_len = ((plen >> 4) & 0x0F) + 1;
                var common_len = plen & 0x0F;

                var nameBuilder = new StringBuilder().Append(last_name.Substring(0, common_len));


                nameBuilder.Append(Encoding.ASCII.GetString(br.ReadBytes(name_len)));
                var name = nameBuilder.ToString();

                last_name = name;
                object v = DecodeValue(mapped.type_format, br);
                count += 1;

                var vdata = new byte[4];
                vdata = br.ReadBytes(mapped.type_len);
                //Array.Copy(data, dataPointer, vdata, 0, mapped.type_len);
                //dataPointer += mapped.type_len;

                double? default_value = null;
                if (with_defaults)
                {
                    if ((flags & 1U) == 0)
                    {
                        default_value = Convert.ToDouble(v);
                    }
                    else
                    {
                        default_value = Convert.ToDouble(DecodeValue(mapped.type_format, br));
                    }
                }


                //list.Add(new MAVLink.MAVLinkParam(name, vdata,
                //    (ptype == 1 ? MAVLink.MAV_PARAM_TYPE.INT8 :
                //        ptype == 2 ? MAVLink.MAV_PARAM_TYPE.INT16 :
                //        ptype == 3 ? MAVLink.MAV_PARAM_TYPE.INT32 :
                //        ptype == 4 ? MAVLink.MAV_PARAM_TYPE.REAL32 : (MAVLink.MAV_PARAM_TYPE)0),
                //    (ptype == 1 ? MAVLink.MAV_PARAM_TYPE.INT8 :
                //        ptype == 2 ? MAVLink.MAV_PARAM_TYPE.INT16 :
                //        ptype == 3 ? MAVLink.MAV_PARAM_TYPE.INT32 :
                //        ptype == 4 ? MAVLink.MAV_PARAM_TYPE.REAL32 : (MAVLink.MAV_PARAM_TYPE)0),
                //        default_value));
            }

            return result;
        }

        public static List<mavlink_param_value_t> Extract(MemoryStream paramFileStream)
        {
            List<mavlink_param_value_t> result = new List<mavlink_param_value_t>();

            BinaryReader br = new BinaryReader(paramFileStream);
            var magic = br.ReadUInt16();
            var num_params = br.ReadUInt16();
            var total_params = br.ReadUInt16();

            if (magic != _Magic && magic != _MmagicWithDefaults)
                return null;

            bool hasDefaultValue = magic == _MmagicWithDefaults;

            byte temp;
            byte[] commonBuffer = new byte[256];
            while (paramFileStream.Position < paramFileStream.Length)
            {
                temp = br.ReadByte();
                if (temp == 0)
                    continue;

                paramFileStream.Seek(-1, SeekOrigin.Current);


                var ptype = br.ReadByte();
                var plen = br.ReadByte();
                var flags = (ptype >> 4) & 0x0F;
                ptype &= 0x0F;

                if (!map.TryGetValue(ptype, out (int type_len, char type_format) mapped))
                    return null;

                mavlink_param_value_t msg = new mavlink_param_value_t();
                var nameLen = ((plen >> 4) & 0x0F) + 1;
                var commonLen = plen & 0x0F;

                msg.param_id = new byte[nameLen + commonLen];
                if (commonLen > 0)
                {
                    Array.Copy(commonBuffer, msg.param_id, commonLen);
                    Array.Copy(br.ReadBytes(nameLen), 0, msg.param_id, commonLen, nameLen);
                }
                else
                    Array.Copy(br.ReadBytes(nameLen), msg.param_id, nameLen);
                Array.Copy(msg.param_id, 0, commonBuffer, 0, nameLen + commonLen);


                Console.WriteLine(Encoding.ASCII.GetString(msg.param_id));
                msg.param_type = (byte)DecodeType(mapped.type_format);
                msg.param_value = Convert.ToSingle(DecodeValue(mapped.type_format, br));

                decimal defaultValue = decimal.MinValue;
                if (hasDefaultValue)
                {
                    if ((flags & 1U) == 0)
                    {
                    }
                    else
                    {
                        defaultValue = Convert.ToDecimal(DecodeValue(mapped.type_format, br));
                    }
                }

                // 不能放到前面，否则 struct 不会被修改
                result.Add(msg);
            }

            return result;
        }

        private static object DecodeValue(char type_format, BinaryReader br)
        {
            switch (type_format)
            {
                case 'b': return br.ReadByte(); 
                case 'h': return br.ReadInt16(); 
                case 'i': return br.ReadInt32(); 
                case 'f': return br.ReadSingle(); 
                default:
                    throw new Exception($"Unexpected type_format: {type_format}");
            }
        }

        private static MAVLink.MAV_PARAM_TYPE DecodeType(char b)
        {
            switch (b)
            {
                case 'b': return MAVLink.MAV_PARAM_TYPE.UINT8;
                case 'h': return MAVLink.MAV_PARAM_TYPE.UINT16;
                case 'i': return MAVLink.MAV_PARAM_TYPE.UINT32;
                case 'f': return MAVLink.MAV_PARAM_TYPE.REAL32;
                default:
                    throw new Exception($"Unexpected type_format: {b}");
            }
        }
    }
}
